/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <https://github.com/raulmur/ORB_SLAM2>
*
* ORB-SLAM2 is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM2 is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/

#include "KeyFrame.h"
#include "Converter.h"
#include "ORBmatcher.h"
#include "MongoTypes.h"
#include <mutex>
#include <include/System.h>

#include "MongoTypes.h"

namespace ORB_SLAM2
{

long unsigned int KeyFrame::nNextId=0;

KeyFrame::KeyFrame(Frame &F, Map *pMap):
    mnId(nNextId++),mnFrameId(F.mnId),  mTimeStamp(F.mTimeStamp), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS),
    mfGridElementWidthInv(F.mfGridElementWidthInv), mfGridElementHeightInv(F.mfGridElementHeightInv),
    mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0),
    mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0), mnBAGlobalForKF(0),
    fx(F.fx), fy(F.fy), cx(F.cx), cy(F.cy), invfx(F.invfx), invfy(F.invfy),
    mbf(F.mbf), mb(F.mb), mThDepth(F.mThDepth), N(F.N), mvKeys(F.mvKeys), mvKeysUn(F.mvKeysUn),
    mvuRight(F.mvuRight), mvDepth(F.mvDepth), mDescriptors(F.mDescriptors.clone()),
    mBowVec(F.mBowVec), mFeatVec(F.mFeatVec), mnScaleLevels(F.mnScaleLevels), mfScaleFactor(F.mfScaleFactor),
    mfLogScaleFactor(F.mfLogScaleFactor), mvScaleFactors(F.mvScaleFactors), mvLevelSigma2(F.mvLevelSigma2),
    mvInvLevelSigma2(F.mvInvLevelSigma2), mnMinX(F.mnMinX), mnMinY(F.mnMinY), mnMaxX(F.mnMaxX),
    mnMaxY(F.mnMaxY), mK(F.mK), mvpMapPoints(F.mvpMapPoints),
    mpORBvocabulary(F.mpORBvocabulary), mbFirstConnection(true), mpParent(NULL), mbNotErase(false),
    mbToBeErased(false), mbBad(false), mHalfBaseline(F.mb/2), mpMap(pMap)
{

    mGrid.resize(mnGridCols);
    for(int i=0; i<mnGridCols;i++)
    {
        mGrid[i].resize(mnGridRows);
        for(int j=0; j<mnGridRows; j++)
            mGrid[i][j] = F.mGrid[i][j];
    }

    SetPose(F.mTcw);
}

KeyFrame::KeyFrame(LoadKeyFrame &F):
    mnId(F.mnId),mnFrameId(F.mnFrameId),  mTimeStamp(F.mTimeStamp), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS),
    mfGridElementWidthInv(F.mfGridElementWidthInv), mfGridElementHeightInv(F.mfGridElementHeightInv),
    mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0),
    mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0), mnBAGlobalForKF(0),
    fx(F.fx), fy(F.fy), cx(F.cx), cy(F.cy), invfx(F.invfx), invfy(F.invfy),
    mbf(F.mbf), mb(F.mb), mThDepth(F.mThDepth), N(F.N), mvKeys(F.mvKeys), mvKeysUn(F.mvKeysUn),
    mvuRight(F.mvuRight), mvDepth(F.mvDepth), mDescriptors(F.mDescriptors.clone()),
    mBowVec(F.mBowVec), mFeatVec(F.mFeatVec), mnScaleLevels(F.mnScaleLevels), mfScaleFactor(F.mfScaleFactor),
    mfLogScaleFactor(F.mfLogScaleFactor), mvScaleFactors(F.mvScaleFactors), mvLevelSigma2(F.mvLevelSigma2),
    mvInvLevelSigma2(F.mvInvLevelSigma2), mnMinX(F.mnMinX), mnMinY(F.mnMinY), mnMaxX(F.mnMaxX),
    mnMaxY(F.mnMaxY), mK(F.mK), mpORBvocabulary(NULL), mGrid(F.mGrid),
    mbFirstConnection(true), mpParent(NULL), mbNotErase(false),
    mbToBeErased(false), mbBad(false), mHalfBaseline(F.mb/2), mpMap(F.mpMap)
{
    SetPose(F.Tcw);

    //! to database to query
    mObejctID = F.mObejctID;

    mpMap->SetDatabaseKeyFrame(this);

    mvpMapPoints.resize(F.mvMapPointsID.size());
    for (size_t i = 0; i < F.mvMapPointsID.size(); ++i) {
        bsoncxx::types::b_oid &oid_mp = F.mvMapPointsID[i];

        MapPoint* pMP = mpMap->GetDatabaseMapPoint(oid_mp);

        if(!pMP)
        {
            bsoncxx::stdx::optional<bsoncxx::document::value> result_mp;
            mpMap->mpMongoIO->FindFromDataBase(Map::sDatabase, Map::sCollectionMP, oid_mp, mongocxx::options::find{}, result_mp);

            if(result_mp)
            {
                bsoncxx::document::view view_mp{result_mp.value()};
                pMP = mpMap->AddMapPoint(view_mp, MongoIO::ID(oid_mp));
            }
        }

        mvpMapPoints[i] = pMP;
    }
}

void KeyFrame::ComputeBoW()
{
    if(mBowVec.empty() || mFeatVec.empty())
    {
        vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
        // Feature vector associate features with nodes in the 4th level (from leaves up)
        // We assume the vocabulary tree has 6 levels, change the 4 otherwise
        mpORBvocabulary->transform(vCurrentDesc,mBowVec,mFeatVec,4);
    }
}

void KeyFrame::SetPose(const cv::Mat &Tcw_)
{
    unique_lock<mutex> lock(mMutexPose);
    Tcw_.copyTo(Tcw);
    cv::Mat Rcw = Tcw.rowRange(0,3).colRange(0,3);
    cv::Mat tcw = Tcw.rowRange(0,3).col(3);
    cv::Mat Rwc = Rcw.t();
    Ow = -Rwc*tcw;

    Twc = cv::Mat::eye(4,4,Tcw.type());
    Rwc.copyTo(Twc.rowRange(0,3).colRange(0,3));
    Ow.copyTo(Twc.rowRange(0,3).col(3));
    cv::Mat center = (cv::Mat_<float>(4,1) << mHalfBaseline, 0 , 0, 1);
    Cw = Twc*center;

    if(mObejctID.status())
    {
        mpMap->AddKeyFrameUpdateBuffer(this, "Tcw");
    }
}

cv::Mat KeyFrame::GetPose()
{
    unique_lock<mutex> lock(mMutexPose);
    return Tcw.clone();
}

cv::Mat KeyFrame::GetPoseInverse()
{
    unique_lock<mutex> lock(mMutexPose);
    return Twc.clone();
}

cv::Mat KeyFrame::GetCameraCenter()
{
    unique_lock<mutex> lock(mMutexPose);
    return Ow.clone();
}

cv::Mat KeyFrame::GetStereoCenter()
{
    unique_lock<mutex> lock(mMutexPose);
    return Cw.clone();
}


cv::Mat KeyFrame::GetRotation()
{
    unique_lock<mutex> lock(mMutexPose);
    return Tcw.rowRange(0,3).colRange(0,3).clone();
}

cv::Mat KeyFrame::GetTranslation()
{
    unique_lock<mutex> lock(mMutexPose);
    return Tcw.rowRange(0,3).col(3).clone();
}

void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight)
{
    {
        unique_lock<mutex> lock(mMutexConnections);
        if(!mConnectedKeyFrameWeights.count(pKF))
            mConnectedKeyFrameWeights[pKF]=weight;
        else if(mConnectedKeyFrameWeights[pKF]!=weight)
            mConnectedKeyFrameWeights[pKF]=weight;
        else
            return;

        if(mObejctID.status())
        {
            mpMap->AddKeyFrameUpdateBuffer(this, "mConnectedKeyFrameWeights");
        }
    }

    UpdateBestCovisibles();
}

void KeyFrame::UpdateBestCovisibles()
{
    unique_lock<mutex> lock(mMutexConnections);
    vector<pair<int,KeyFrame*> > vPairs;
    vPairs.reserve(mConnectedKeyFrameWeights.size());
    for(map<KeyFrame*,int>::iterator mit=mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++)
       vPairs.push_back(make_pair(mit->second,mit->first));

    sort(vPairs.begin(),vPairs.end());
    list<KeyFrame*> lKFs;
    list<int> lWs;
    for(size_t i=0, iend=vPairs.size(); i<iend;i++)
    {
        lKFs.push_front(vPairs[i].second);
        lWs.push_front(vPairs[i].first);
    }

    mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
    mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());

    if(mObejctID.status())
    {
        mpMap->AddKeyFrameUpdateBuffer(this, "mvpOrderedConnectedKeyFrames");
        mpMap->AddKeyFrameUpdateBuffer(this, "mvOrderedWeights");
    }
}

set<KeyFrame*> KeyFrame::GetConnectedKeyFrames()
{
    unique_lock<mutex> lock(mMutexConnections);
    set<KeyFrame*> s;
    for(map<KeyFrame*,int>::iterator mit=mConnectedKeyFrameWeights.begin();mit!=mConnectedKeyFrameWeights.end();mit++)
        s.insert(mit->first);
    return s;
}

vector<KeyFrame*> KeyFrame::GetVectorCovisibleKeyFrames()
{
    unique_lock<mutex> lock(mMutexConnections);
    return mvpOrderedConnectedKeyFrames;
}

vector<KeyFrame*> KeyFrame::GetBestCovisibilityKeyFrames(const int &N)
{
    unique_lock<mutex> lock(mMutexConnections);
    if((int)mvpOrderedConnectedKeyFrames.size()<N)
        return mvpOrderedConnectedKeyFrames;
    else
        return vector<KeyFrame*>(mvpOrderedConnectedKeyFrames.begin(),mvpOrderedConnectedKeyFrames.begin()+N);

}

vector<KeyFrame*> KeyFrame::GetCovisiblesByWeight(const int &w)
{
    unique_lock<mutex> lock(mMutexConnections);

    if(mvpOrderedConnectedKeyFrames.empty())
        return vector<KeyFrame*>();

    vector<int>::iterator it = upper_bound(mvOrderedWeights.begin(),mvOrderedWeights.end(),w,KeyFrame::weightComp);
    if(it==mvOrderedWeights.end())
        return vector<KeyFrame*>();
    else
    {
        int n = it-mvOrderedWeights.begin();
        return vector<KeyFrame*>(mvpOrderedConnectedKeyFrames.begin(), mvpOrderedConnectedKeyFrames.begin()+n);
    }
}

int KeyFrame::GetWeight(KeyFrame *pKF)
{
    unique_lock<mutex> lock(mMutexConnections);
    if(mConnectedKeyFrameWeights.count(pKF))
        return mConnectedKeyFrameWeights[pKF];
    else
        return 0;
}

void KeyFrame::AddMapPoint(MapPoint *pMP, const size_t &idx)
{
    unique_lock<mutex> lock(mMutexFeatures);
    mvpMapPoints[idx]=pMP;

    if(mObejctID.status())
    {
        mpMap->AddKeyFrameUpdateBuffer(this, "mvpMapPoints");
    }
}

void KeyFrame::EraseMapPointMatch(const size_t &idx)
{
    unique_lock<mutex> lock(mMutexFeatures);
    mvpMapPoints[idx]=static_cast<MapPoint*>(NULL);

    if(mObejctID.status())
    {
        mpMap->AddKeyFrameUpdateBuffer(this, "mvpMapPoints");

    }
}

void KeyFrame::EraseMapPointMatch(MapPoint* pMP)
{
    int idx = pMP->GetIndexInKeyFrame(this);
    if(idx>=0)
    {
        mvpMapPoints[idx]=static_cast<MapPoint*>(NULL);

        if(mObejctID.status()) {
            mpMap->AddKeyFrameUpdateBuffer(this, "mvpMapPoints");
        }
    }
}


void KeyFrame::ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP)
{
    mvpMapPoints[idx]=pMP;

    if(mObejctID.status())
    {
        mpMap->AddKeyFrameUpdateBuffer(this, "mvpMapPoints");
    }
}

set<MapPoint*> KeyFrame::GetMapPoints()
{
    unique_lock<mutex> lock(mMutexFeatures);
    set<MapPoint*> s;
    for(size_t i=0, iend=mvpMapPoints.size(); i<iend; i++)
    {
        if(!mvpMapPoints[i])
            continue;
        MapPoint* pMP = mvpMapPoints[i];
        if(!pMP->isBad())
            s.insert(pMP);
    }
    return s;
}

int KeyFrame::TrackedMapPoints(const int &minObs)
{
    unique_lock<mutex> lock(mMutexFeatures);

    int nPoints=0;
    const bool bCheckObs = minObs>0;
    for(int i=0; i<N; i++)
    {
        MapPoint* pMP = mvpMapPoints[i];
        if(pMP)
        {
            if(!pMP->isBad())
            {
                if(bCheckObs)
                {
                    if(mvpMapPoints[i]->Observations()>=minObs)
                        nPoints++;
                }
                else
                    nPoints++;
            }
        }
    }

    return nPoints;
}

vector<MapPoint*> KeyFrame::GetMapPointMatches()
{
    unique_lock<mutex> lock(mMutexFeatures);
    return mvpMapPoints;
}

MapPoint* KeyFrame::GetMapPoint(const size_t &idx)
{
    unique_lock<mutex> lock(mMutexFeatures);
    return mvpMapPoints[idx];
}

void KeyFrame::UpdateConnections()
{
    map<KeyFrame*,int> KFcounter;

    vector<MapPoint*> vpMP;

    {
        unique_lock<mutex> lockMPs(mMutexFeatures);
        vpMP = mvpMapPoints;
    }

    //For all map points in keyframe check in which other keyframes are they seen
    //Increase counter for those keyframes
    for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)
    {
        MapPoint* pMP = *vit;

        if(!pMP)
            continue;

        if(pMP->isBad())
            continue;

        map<KeyFrame*,size_t> observations = pMP->GetObservations();

        for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
        {
            if(mit->first->mnId==mnId)
                continue;
            KFcounter[mit->first]++;
        }
    }

    // This should not happen
    if(KFcounter.empty())
        return;

    //If the counter is greater than threshold add connection
    //In case no keyframe counter is over threshold add the one with maximum counter
    int nmax=0;
    KeyFrame* pKFmax=NULL;
    int th = 15;

    vector<pair<int,KeyFrame*> > vPairs;
    vPairs.reserve(KFcounter.size());
    for(map<KeyFrame*,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++)
    {
        if(mit->second>nmax)
        {
            nmax=mit->second;
            pKFmax=mit->first;
        }
        if(mit->second>=th)
        {
            vPairs.push_back(make_pair(mit->second,mit->first));
            (mit->first)->AddConnection(this,mit->second);
        }
    }

    if(vPairs.empty())
    {
        vPairs.push_back(make_pair(nmax,pKFmax));
        pKFmax->AddConnection(this,nmax);
    }

    sort(vPairs.begin(),vPairs.end());
    list<KeyFrame*> lKFs;
    list<int> lWs;
    for(size_t i=0; i<vPairs.size();i++)
    {
        lKFs.push_front(vPairs[i].second);
        lWs.push_front(vPairs[i].first);
    }

    {
        unique_lock<mutex> lockCon(mMutexConnections);

        // mspConnectedKeyFrames = spConnectedKeyFrames;
        mConnectedKeyFrameWeights = KFcounter;
        mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
        mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());

        mpMap->AddKeyFrameUpdateBuffer(this, "mConnectedKeyFrameWeights");
        mpMap->AddKeyFrameUpdateBuffer(this, "mvpOrderedConnectedKeyFrames");
        mpMap->AddKeyFrameUpdateBuffer(this, "mvOrderedWeights");

        if(mbFirstConnection && mnId!=0)
        {
            mpParent = mvpOrderedConnectedKeyFrames.front();
            mpParent->AddChild(this);
            mbFirstConnection = false;

            mpMap->AddKeyFrameUpdateBuffer(this, "mpParent");
            mpMap->AddKeyFrameUpdateBuffer(this, "mbFirstConnection");
        }

    }
}

void KeyFrame::AddChild(KeyFrame *pKF)
{
    unique_lock<mutex> lockCon(mMutexConnections);
    mspChildrens.insert(pKF);

    if(mObejctID.status()) {
        mpMap->AddKeyFrameUpdateBuffer(this, "mspChildrens");
    }
}

void KeyFrame::EraseChild(KeyFrame *pKF)
{
    unique_lock<mutex> lockCon(mMutexConnections);
    mspChildrens.erase(pKF);

    if(mObejctID.status()) {
        mpMap->AddKeyFrameUpdateBuffer(this, "mspChildrens");
    }
}

void KeyFrame::ChangeParent(KeyFrame *pKF)
{
    unique_lock<mutex> lockCon(mMutexConnections);
    mpParent = pKF;

    pKF->AddChild(this);

    if(mObejctID.status()) {
        mpMap->AddKeyFrameUpdateBuffer(this, "mpParent");
    }
}

set<KeyFrame*> KeyFrame::GetChilds()
{
    unique_lock<mutex> lockCon(mMutexConnections);
    return mspChildrens;
}

KeyFrame* KeyFrame::GetParent()
{
    unique_lock<mutex> lockCon(mMutexConnections);
    return mpParent;
}

bool KeyFrame::hasChild(KeyFrame *pKF)
{
    unique_lock<mutex> lockCon(mMutexConnections);
    return mspChildrens.count(pKF);
}

void KeyFrame::AddLoopEdge(KeyFrame *pKF)
{
    unique_lock<mutex> lockCon(mMutexConnections);
    mbNotErase = true;
    mspLoopEdges.insert(pKF);

    if(mObejctID.status()) {
        mpMap->AddKeyFrameUpdateBuffer(this, "mbNotErase");
        mpMap->AddKeyFrameUpdateBuffer(this, "mspLoopEdges");
    }
}

set<KeyFrame*> KeyFrame::GetLoopEdges()
{
    unique_lock<mutex> lockCon(mMutexConnections);
    return mspLoopEdges;
}

void KeyFrame::SetNotErase()
{
    unique_lock<mutex> lock(mMutexConnections);
    mbNotErase = true;

    if(mObejctID.status()) {
        mpMap->AddKeyFrameUpdateBuffer(this, "mbNotErase");
    }
}

void KeyFrame::SetErase()
{
    {
        unique_lock<mutex> lock(mMutexConnections);
        if(mspLoopEdges.empty())
        {
            mbNotErase = false;

            if(mObejctID.status()) {
                mpMap->AddKeyFrameUpdateBuffer(this, "mbNotErase");
            }
        }
    }

    if(mbToBeErased)
    {
        SetBadFlag();
    }
}

void KeyFrame::SetBadFlag()
{
    {
        unique_lock<mutex> lock(mMutexConnections);
        if(mnId==0)
            return;
        else if(mbNotErase)
        {
            mbToBeErased = true;

            if(mObejctID.status()) {
                mpMap->AddKeyFrameUpdateBuffer(this, "mbToBeErased");
            }
            return;
        }
    }

    for(map<KeyFrame*,int>::iterator mit = mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++)
        mit->first->EraseConnection(this);

    for(size_t i=0; i<mvpMapPoints.size(); i++)
        if(mvpMapPoints[i])
            mvpMapPoints[i]->EraseObservation(this);
    {
        unique_lock<mutex> lock(mMutexConnections);
        unique_lock<mutex> lock1(mMutexFeatures);

        mConnectedKeyFrameWeights.clear();
        mvpOrderedConnectedKeyFrames.clear();

        // Update Spanning Tree
        set<KeyFrame*> sParentCandidates;
        sParentCandidates.insert(mpParent);

        // Assign at each iteration one children with a parent (the pair with highest covisibility weight)
        // Include that children as new parent candidate for the rest
        while(!mspChildrens.empty())
        {
            bool bContinue = false;

            int max = -1;
            KeyFrame* pC;
            KeyFrame* pP;

            for(set<KeyFrame*>::iterator sit=mspChildrens.begin(), send=mspChildrens.end(); sit!=send; sit++)
            {
                KeyFrame* pKF = *sit;
                if(pKF->isBad())
                    continue;

                // Check if a parent candidate is connected to the keyframe
                vector<KeyFrame*> vpConnected = pKF->GetVectorCovisibleKeyFrames();
                for(size_t i=0, iend=vpConnected.size(); i<iend; i++)
                {
                    for(set<KeyFrame*>::iterator spcit=sParentCandidates.begin(), spcend=sParentCandidates.end(); spcit!=spcend; spcit++)
                    {
                        if(vpConnected[i]->mnId == (*spcit)->mnId)
                        {
                            int w = pKF->GetWeight(vpConnected[i]);
                            if(w>max)
                            {
                                pC = pKF;
                                pP = vpConnected[i];
                                max = w;
                                bContinue = true;
                            }
                        }
                    }
                }
            }

            if(bContinue)
            {
                pC->ChangeParent(pP);
                sParentCandidates.insert(pC);
                mspChildrens.erase(pC);
            }
            else
                break;
        }

        // If a children has no covisibility links with any parent candidate, assign to the original parent of this KF
        if(!mspChildrens.empty())
            for(set<KeyFrame*>::iterator sit=mspChildrens.begin(); sit!=mspChildrens.end(); sit++)
            {
                (*sit)->ChangeParent(mpParent);
            }

        mpParent->EraseChild(this);
        mTcp = Tcw*mpParent->GetPoseInverse();
        mbBad = true;
    }

    //! 直接删除数据，不需要对其他变量更新了
    mpMap->EraseKeyFrame(this);

    // !rain
    // it shoulds to inform the loopclosing thread and synchro the keyframe database
    mpMap->mpKeyFrameDB->erase(this);
}

bool KeyFrame::isBad()
{
    unique_lock<mutex> lock(mMutexConnections);
    return mbBad;
}

void KeyFrame::EraseConnection(KeyFrame* pKF)
{
    bool bUpdate = false;
    {
        unique_lock<mutex> lock(mMutexConnections);
        if(mConnectedKeyFrameWeights.count(pKF))
        {
            mConnectedKeyFrameWeights.erase(pKF);
            bUpdate=true;

            if(mObejctID.status()) {
                mpMap->AddKeyFrameUpdateBuffer(this, "mConnectedKeyFrameWeights");
            }
        }
    }

    if(bUpdate)
        UpdateBestCovisibles();
}

vector<size_t> KeyFrame::GetFeaturesInArea(const float &x, const float &y, const float &r) const
{
    vector<size_t> vIndices;
    vIndices.reserve(N);

    const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv));
    if(nMinCellX>=mnGridCols)
        return vIndices;

    const int nMaxCellX = min((int)mnGridCols-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv));
    if(nMaxCellX<0)
        return vIndices;

    const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv));
    if(nMinCellY>=mnGridRows)
        return vIndices;

    const int nMaxCellY = min((int)mnGridRows-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv));
    if(nMaxCellY<0)
        return vIndices;

    for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
    {
        for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
        {
            const vector<size_t> vCell = mGrid[ix][iy];
            for(size_t j=0, jend=vCell.size(); j<jend; j++)
            {
                const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]];
                const float distx = kpUn.pt.x-x;
                const float disty = kpUn.pt.y-y;

                if(fabs(distx)<r && fabs(disty)<r)
                    vIndices.push_back(vCell[j]);
            }
        }
    }

    return vIndices;
}

bool KeyFrame::IsInImage(const float &x, const float &y) const
{
    return (x>=mnMinX && x<mnMaxX && y>=mnMinY && y<mnMaxY);
}

cv::Mat KeyFrame::UnprojectStereo(int i)
{
    const float z = mvDepth[i];
    if(z>0)
    {
        const float u = mvKeys[i].pt.x;
        const float v = mvKeys[i].pt.y;
        const float x = (u-cx)*z*invfx;
        const float y = (v-cy)*z*invfy;
        cv::Mat x3Dc = (cv::Mat_<float>(3,1) << x, y, z);

        unique_lock<mutex> lock(mMutexPose);
        return Twc.rowRange(0,3).colRange(0,3)*x3Dc+Twc.rowRange(0,3).col(3);
    }
    else
        return cv::Mat();
}

float KeyFrame::ComputeSceneMedianDepth(const int q)
{
    vector<MapPoint*> vpMapPoints;
    cv::Mat Tcw_;
    {
        unique_lock<mutex> lock(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPose);
        vpMapPoints = mvpMapPoints;
        Tcw_ = Tcw.clone();
    }

    vector<float> vDepths;
    vDepths.reserve(N);
    cv::Mat Rcw2 = Tcw_.row(2).colRange(0,3);
    Rcw2 = Rcw2.t();
    float zcw = Tcw_.at<float>(2,3);
    for(int i=0; i<N; i++)
    {
        if(mvpMapPoints[i])
        {
            MapPoint* pMP = mvpMapPoints[i];
            cv::Mat x3Dw = pMP->GetWorldPos();
            float z = Rcw2.dot(x3Dw)+zcw;
            vDepths.push_back(z);
        }
    }

    sort(vDepths.begin(),vDepths.end());

    return vDepths[(vDepths.size()-1)/q];
}

/*
 * Database
 */
const bsoncxx::types::b_oid KeyFrame::WriteToDataBase()
{
    //! Write unchanged parameters
    bsoncxx::builder::stream::document docKF;
    docKF << "mnId" << bsoncxx::types::b_int64{static_cast<int64_t>(mnId)}
          << "mnFrameId" << bsoncxx::types::b_int64{static_cast<int64_t>(mnFrameId)}
          << "mTimeStamp" << mTimeStamp;

    // Grid (to speed up feature matching)
    docKF //<< "mnGridCols" << mnGridCols
          //<< "mnGridRows" << mnGridRows
          << "mfGridElementWidthInv" << mfGridElementWidthInv
          << "mfGridElementHeightInv" << mfGridElementHeightInv

          // Calibration parameters
          << "fx" << fx << "fy" << fy << "cx" << cx << "cy" << cy
          << "invfx" << invfx << "invfy" << invfy
          << "mbf" << mbf << "mb" << mb << "mThDepth" << mThDepth

          // Number of KeyPoints
          << "N" << N

          // KeyPoints, stereo coordinate and descriptors (all associated by an index)
          << MongoIO::types::b_vector_keypoint{"mvKeys", mvKeys}
          << MongoIO::types::b_vector_keypoint{"mvKeysUn", mvKeysUn}
          << MongoIO::types::b_vector_float{"mvuRight", mvuRight}
          << MongoIO::types::b_vector_float{"mvDepth", mvDepth}
          << MongoIO::types::b_mat{"mDescriptors", mDescriptors}

          // Scale
          << "mnScaleLevels" << mnScaleLevels
          << "mfScaleFactor" << mfScaleFactor
          << "mfLogScaleFactor" << mfLogScaleFactor

          << MongoIO::types::b_vector_float{"mvScaleFactors", mvScaleFactors}
          << MongoIO::types::b_vector_float{"mvLevelSigma2", mvLevelSigma2}
          << MongoIO::types::b_vector_float{"mvInvLevelSigma2", mvInvLevelSigma2}

          // Image bounds and calibration
          << "mnMinX" << mnMinX
          << "mnMinY" << mnMinY
          << "mnMaxX" << mnMaxX
          << "mnMaxY" << mnMaxY
          << MongoIO::types::b_mat{"mK", mK};

    docKF << MongoIO::types::b_bowvec{"mBowVec", mBowVec}
          << MongoIO::types::b_bowfeat{"mFeatVec", mFeatVec};

    docKF << MongoIO::types::b_mat{"Tcw", Tcw}
          << MongoIO::types::b_grid{"mGrid", mGrid};

    //! 正常在初始化的时候写入，这里都是空的
    std::vector<std::pair<bsoncxx::types::b_oid, int> > ConnectedKeyFrameWeights;
    for_each(mConnectedKeyFrameWeights.begin(), mConnectedKeyFrameWeights.end(), [&](std::pair<KeyFrame*, int> val){
        if(!val.first->mObejctID.status())
            std::cerr << "Error in writing of KeyFrame::mConnectedKeyFrameWeights!!! The KeyFrame " << val.first->mnId << " is not in the database!!!" << std::endl;
        ConnectedKeyFrameWeights.push_back(std::pair<bsoncxx::types::b_oid, int>(val.first->mObejctID.oid(), val.second));
    });

    std::vector<bsoncxx::types::b_oid> vpOrderedConnectedKeyFrames;
    for_each(mvpOrderedConnectedKeyFrames.begin(), mvpOrderedConnectedKeyFrames.end(), [&](KeyFrame* val){
        if(!val->mObejctID.status())
          std::cerr << "Error int writing of KeyFrame::mvpOrderedConnectedKeyFrames!!! The KeyFrame " << val->mnId << " is not in the database!!!" << std::endl;
        vpOrderedConnectedKeyFrames.push_back(val->mObejctID.oid());
    });

    docKF << MongoIO::types::b_vector<std::pair<bsoncxx::types::b_oid, int> >{"mConnectedKeyFrameWeights", ConnectedKeyFrameWeights}
          << MongoIO::types::b_vector<bsoncxx::types::b_oid>{"mvpOrderedConnectedKeyFrames", vpOrderedConnectedKeyFrames}
          << MongoIO::types::b_vector<int>{"mvOrderedWeights", mvOrderedWeights};


    std::vector<bsoncxx::types::b_oid> spChildrens;
    for_each(mspChildrens.begin(), mspChildrens.end(), [&](KeyFrame* val){
        if(!val->mObejctID.status())
          std::cerr << "Error in writing of KeyFrame::mspChildrens!!! The KeyFrame " << val->mnId << " is not in the database!!!" << std::endl;
        spChildrens.push_back(val->mObejctID.oid());
    });

    std::vector<bsoncxx::types::b_oid> spLoopEdges;
    for_each(mspLoopEdges.begin(), mspLoopEdges.end(), [&](KeyFrame* val){
      if(!val->mObejctID.status())
          std::cerr << "Error in writing of KeyFrame::mspLoopEdges!!! The KeyFrame " << val->mnId << " is not in the database!!!" << std::endl;
      spLoopEdges.push_back(val->mObejctID.oid());
    });

    docKF << "mbFirstConnection" << mbFirstConnection;

    if(mpParent)
        docKF<< "mpParent" << mpParent->mObejctID.oid();
    else
        docKF<< "mpParent" << bsoncxx::types::b_oid();

    docKF << MongoIO::types::b_vector<bsoncxx::types::b_oid>{"mspChildrens", spChildrens}
          << MongoIO::types::b_vector<bsoncxx::types::b_oid>{"mspLoopEdges", spLoopEdges};

    docKF << "mbNotErase" << mbNotErase
          << "mbToBeErased" << mbToBeErased
          << "mbBad" << mbBad;

    docKF << "mHalfBaseline" << mHalfBaseline;

    std::vector<bsoncxx::types::b_oid> vpMapPointsID;
    docKF << MongoIO::types::b_vector<bsoncxx::types::b_oid>{"mvpMapPoints", vpMapPointsID};

    //! Write to DataBase && Get ObejctID
    bsoncxx::types::b_oid oid = mpMap->mpMongoIO->WriteToDataBase(Map::sDatabase, Map::sCollectionKF, docKF.view());
    mObejctID = MongoIO::ID(oid);

    docKF.clear();

    //! Write MapPoint
    vpMapPointsID.resize(mvpMapPoints.size());
    for(size_t i = 0; i < mvpMapPoints.size(); i++)
    {
        MapPoint* pMP = mvpMapPoints[i];
        if(!pMP)
        {
            continue;
        }
        else if(pMP->MongoID().status()){
            vpMapPointsID[i] = pMP->MongoID().oid();
        } else{

            std::cerr << "MapPoint " << pMP->mnId << " in KeyFrame " << mnId << "is not in database" << std::endl;
        }
    }

    docKF << MongoIO::types::b_vector<bsoncxx::types::b_oid>{"mvpMapPoints", vpMapPointsID};
    mpMap->mpMongoIO->UpdateToDataBase(Map::sDatabase, Map::sCollectionKF, oid, docKF.view());

    return oid;
}

bool KeyFrame::EraseFromDataBase()
{
    const bsoncxx::types::b_oid oid = mObejctID.oid();
    mObejctID = MongoIO::ID();

    return mpMap->mpMongoIO->EraseFromDataBase(Map::sDatabase, Map::sCollectionKF, oid);
}

bool KeyFrame::UpdateToDataBase(const std::set<std::string> &paras)
{
    if(!mObejctID.status()) {
        std::cerr << " Unvaluable KeyFrame: " << mnId << ", UpdateToDataBase rejected"  << std::endl;
        return false;
    }

    bsoncxx::builder::stream::document update_builder{};

    for_each(paras.begin(), paras.end(), [&](const std::string para){
        if("Tcw" == para)
        {
            update_builder <<  MongoIO::types::b_mat{"Tcw", Tcw};
        }
        else if("mpParent" == para)
        {
            if(mpParent)
                update_builder<< "mpParent" << mpParent->mObejctID.oid();
            else
                update_builder<< "mpParent" << bsoncxx::types::b_oid();
        }
        else if("mbFirstConnection" == para)
        {
            update_builder<< "mbFirstConnection" << mbFirstConnection;
        }
        else if("mvpMapPoints" == para)
        {
            std::vector<bsoncxx::types::b_oid> vpMapPointsID(mvpMapPoints.size());
            for(size_t i = 0; i < mvpMapPoints.size(); i++)
            {
                MapPoint* pMP = mvpMapPoints[i];
                if(!pMP)
                {
                    continue;
                }
                else if(pMP->MongoID().status()){
                    vpMapPointsID[i] = pMP->MongoID().oid();
                } else{

                    std::cerr << "MapPoint " << pMP->mnId << " in KeyFrame " << mnId << "is not in database" << std::endl;
                }
            }

            update_builder << MongoIO::types::b_vector<bsoncxx::types::b_oid>{"mvpMapPoints", vpMapPointsID};
        }
        else if("mConnectedKeyFrameWeights" == para)
        {
            std::vector<std::pair<bsoncxx::types::b_oid, int> > ConnectedKeyFrameWeights;
            for_each(mConnectedKeyFrameWeights.begin(), mConnectedKeyFrameWeights.end(), [&](std::pair<KeyFrame*, int> val){
              if(!val.first->mObejctID.status())
                  std::cerr << "Error in writing of KeyFrame::mConnectedKeyFrameWeights!!! The KeyFrame " << val.first->mnId << " is not in the database!!!" << std::endl;
              ConnectedKeyFrameWeights.push_back(std::pair<bsoncxx::types::b_oid, int>(val.first->mObejctID.oid(), val.second));
            });

            update_builder << MongoIO::types::b_vector<std::pair<bsoncxx::types::b_oid, int> >{"mConnectedKeyFrameWeights", ConnectedKeyFrameWeights};
        }
        else if("mvpOrderedConnectedKeyFrames" == para)
        {
            std::vector<bsoncxx::types::b_oid> vpOrderedConnectedKeyFrames;
            for_each(mvpOrderedConnectedKeyFrames.begin(), mvpOrderedConnectedKeyFrames.end(), [&](KeyFrame* val){
              if(!val->mObejctID.status())
                  std::cerr << "Error int writing of KeyFrame::mvpOrderedConnectedKeyFrames!!! The KeyFrame " << val->mnId << " is not in the database!!!" << std::endl;
              vpOrderedConnectedKeyFrames.push_back(val->mObejctID.oid());
            });

            update_builder << MongoIO::types::b_vector<bsoncxx::types::b_oid>{"mvpOrderedConnectedKeyFrames", vpOrderedConnectedKeyFrames};
        }
        else if("mvOrderedWeights" == para)
        {
            update_builder << MongoIO::types::b_vector<int>{"mvOrderedWeights", mvOrderedWeights};
        }
        else if("mspChildrens" == para)
        {
            std::vector<bsoncxx::types::b_oid> spChildrens;
            for_each(mspChildrens.begin(), mspChildrens.end(), [&](KeyFrame* val){
              if(!val->mObejctID.status())
                  std::cerr << "Error in writing of KeyFrame::mspChildrens!!! The KeyFrame " << val->mnId << " is not in the database!!!" << std::endl;
              spChildrens.push_back(val->mObejctID.oid());
            });

            update_builder << MongoIO::types::b_vector<bsoncxx::types::b_oid>{"mspChildrens", spChildrens};

        }
        else if("mspLoopEdges" == para)
        {
            std::vector<bsoncxx::types::b_oid> spLoopEdges;
            for_each(mspLoopEdges.begin(), mspLoopEdges.end(), [&](KeyFrame* val){
              if(!val->mObejctID.status())
                  std::cerr << "Error in writing of KeyFrame::mspLoopEdges!!! The KeyFrame " << val->mnId << " is not in the database!!!" << std::endl;
              spLoopEdges.push_back(val->mObejctID.oid());
            });

            update_builder << MongoIO::types::b_vector<bsoncxx::types::b_oid>{"mspLoopEdges", spLoopEdges};
        }
        else if("mbNotErase" == para)
        {
            update_builder << "mbNotErase" << mbNotErase;
        }
        else if("mbToBeErased" == para)
        {
            update_builder << "mbToBeErased" << mbToBeErased;
        }
        else if("mbBad" == para)
        {
            update_builder << "mbBad" << mbBad;
        }
        else
        {
            std::cerr << "KeyFrame::UpdateToDataBase Unknow parameter: " << para << std::endl;
            return false;
        }

    });

    mpMap->mpMongoIO->UpdateToDataBase(Map::sDatabase, Map::sCollectionKF, mObejctID.oid(), update_builder.view());

    return true;
}

bool KeyFrame::UpdateFromDataBase(const std::set<std::string> &paras)
{
    if(!mObejctID.status())
    {
        std::cerr << " Unvaluable KeyFrame: " << mnId << ", UpdateFromDataBase rejected"  << std::endl;
        return false;
    }

    mongocxx::options::find opts{};
    bsoncxx::builder::stream::document opts_builder{};
    for_each(paras.begin(), paras.end(), [&](const std::string para){
        opts_builder<< para << 1;
    });
    opts.projection(opts_builder.view());

    bsoncxx::stdx::optional<bsoncxx::document::value> result;
    mpMap->mpMongoIO->FindFromDataBase(Map::sDatabase, Map::sCollectionKF, mObejctID.oid(), opts, result);

    if(!result)
    {
        cout << "rain can't find the keyframe " << mObejctID.oid().value.to_string()<< endl;
        return false;
    }

    bsoncxx::document::view view{result.value().view()};
//    std::cout << "find: " << bsoncxx::to_json(view) << std::endl;

    for_each(paras.begin(), paras.end(), [&](const std::string para) {
        if ("Tcw" == para) {
            cv::Mat T = MongoIO::types::b_mat::from(view["Tcw"]);
            SetPose(T);
        }
        else if ("mpParent" == para) {
            const bsoncxx::types::b_oid pParentID = view["mpParent"].get_oid();
            mpParent = mpMap->GetDatabaseKeyFrame(pParentID);
        }
        else if("mbFirstConnection" == para) {
            mbFirstConnection = view["mbFirstConnection"].get_bool();
        }
        else if ("mvpMapPoints" == para) {
            std::vector<bsoncxx::types::b_oid>
              vMapPointsID = MongoIO::types::b_vector<bsoncxx::types::b_oid>::from(view["mvpMapPoints"]);

            if (N == vMapPointsID.size())
            {
              for (size_t i = 0; i < N; ++i)
              {
                  mvpMapPoints[i] = mpMap->GetDatabaseMapPoint(vMapPointsID[i]);
              }
            }
            else
            {
              std::cerr << "Uncomparable size of mvpMapPoints" << std::endl;
              return false;
            }
        }
        else if("mConnectedKeyFrameWeights" == para) {
            KeyFrame::GetFromDatabase(mConnectedKeyFrameWeights, mpMap, view["mConnectedKeyFrameWeights"]);
        }
        else if ("mvpOrderedConnectedKeyFrames" == para) {
            KeyFrame::GetFromDatabase(mvpOrderedConnectedKeyFrames, mpMap, view["mvpOrderedConnectedKeyFrames"]);
        }
        else if ("mvOrderedWeights" == para) {
            mvOrderedWeights = MongoIO::types::b_vector<int>::from(view["mvOrderedWeights"]);
        }
        else if ("mspChildrens" == para) {
            KeyFrame::GetFromDatabase(mspChildrens, mpMap, view["mspChildrens"]);
        }
        else if ("mspLoopEdges" == para) {
            KeyFrame::GetFromDatabase(mspLoopEdges, mpMap, view["mspLoopEdges"]);
        }
        else if ("mbNotErase" == para) {
            mbNotErase = view["mbNotErase"].get_bool();
        }
        else if ("mbToBeErased" == para) {
            mbToBeErased = view["mbToBeErased"].get_bool();
        }
        else if ("mbBad" == para) {
            mbBad = view["mbBad"].get_bool();
        }
        else {
            std::cerr << "KeyFrame::UpdateToDataBase Unknow parameter: " << para << std::endl;
            return false;
        }
    });

    return true;
}


void KeyFrame::GetFromDatabase(std::map<KeyFrame*, int> &mpKFn, Map* map, const bsoncxx::document::element &elem)
{
    std::vector<std::pair<bsoncxx::types::b_oid, int> > vOidn = MongoIO::types::b_vector<std::pair<bsoncxx::types::b_oid, int> >::from(elem);

    std::for_each(vOidn.begin(), vOidn.end(), [&](const std::pair<bsoncxx::types::b_oid, int> val){
        const bsoncxx::types::b_oid key = val.first;
        const int weight  = val.second;

        KeyFrame* pKF = map->GetDatabaseKeyFrame(key);
        if(pKF) {
            mpKFn.insert(std::pair<KeyFrame*, int>(pKF, weight));
        } else{
        //    std::cerr << "\nError!!! KeyFrame::GetFromDatabase" << elem.key().to_string()
        //            << " Can not find the oid[" << key.value.to_string() << "]" << std::endl;
        }
    });
}

void KeyFrame::GetFromDatabase(std::vector<KeyFrame*> &vpKF, Map* map, const bsoncxx::document::element &elem)
{
    std::vector<bsoncxx::types::b_oid> conect_keyframe = MongoIO::types::b_vector<bsoncxx::types::b_oid>::from(elem);
    for_each(conect_keyframe.begin(), conect_keyframe.end(), [&](bsoncxx::types::b_oid val){
        KeyFrame* pKF = map->GetDatabaseKeyFrame(val);
        if(pKF){
            vpKF.push_back(pKF);
        } else{
        //    std::cerr << "\nError!!! KeyFrame::GetFromDatabase" << elem.key().to_string()
        //              << " Can not find the oid[" << val.value.to_string() << "]" << std::endl;
        }
    });
}

void KeyFrame::GetFromDatabase(std::set<KeyFrame*> &sKF, Map *map, const bsoncxx::document::element &elem)
{
    std::vector<bsoncxx::types::b_oid> oids = MongoIO::types::b_vector<bsoncxx::types::b_oid>::from(elem);

    for_each(oids.begin(), oids.end(), [&](bsoncxx::types::b_oid val){
        KeyFrame* pKF = map->GetDatabaseKeyFrame(val);
        if(pKF){
            sKF.insert(pKF);
        } else{
        //    std::cerr << "\nError!!! KeyFrame::GetFromDatabase" << elem.key().to_string()
        //              << " Can not find the oid[" << val.value.to_string() << "]" << std::endl;
        }
    });
}

LoadKeyFrame::LoadKeyFrame(const MongoIO::ID &id, Map* pMap) : mObejctID(id), mpMap(pMap)
{
    bsoncxx::stdx::optional<bsoncxx::document::value> result;

    pMap->mpMongoIO->FindFromDataBase(Map::sDatabase, Map::sCollectionKF, id.oid(), mongocxx::options::find{}, result);

    bsoncxx::document::view view{result.value()};

    //std::cout << "v " << bsoncxx::to_json(view) << std::endl;

    mnId = static_cast<long unsigned int>(view["mnId"].get_int64());
    mnFrameId = static_cast<long unsigned int>(view["mnFrameId"].get_int64());
    mTimeStamp = view["mTimeStamp"].get_double();

    mfGridElementWidthInv = static_cast<float>(view["mfGridElementWidthInv"].get_double());
    mfGridElementHeightInv = static_cast<float>(view["mfGridElementHeightInv"].get_double());

    fx = static_cast<float>(view["fx"].get_double());
    fy = static_cast<float>(view["fy"].get_double());
    cx = static_cast<float>(view["cx"].get_double());
    cy = static_cast<float>(view["cy"].get_double());

    invfx = static_cast<float>(view["invfx"].get_double());
    invfy = static_cast<float>(view["invfy"].get_double());

    mbf = static_cast<float>(view["mbf"].get_double());
    mb = static_cast<float>(view["mb"].get_double());
    mThDepth = static_cast<float>(view["mThDepth"].get_double());

    N = view["N"].get_int32();

    mvKeys = MongoIO::types::b_vector_keypoint::from(view["mvKeys"]);
    mvKeysUn = MongoIO::types::b_vector_keypoint::from(view["mvKeysUn"]);
    mvuRight = MongoIO::types::b_vector_float::from(view["mvuRight"]);
    mvDepth = MongoIO::types::b_vector_float::from(view["mvDepth"]);
    mDescriptors = MongoIO::types::b_mat::from(view["mDescriptors"]);

    mnScaleLevels = view["mnScaleLevels"].get_int32();
    mfScaleFactor = static_cast<float>(view["mfScaleFactor"].get_double());
    mfLogScaleFactor = static_cast<float>(view["mfLogScaleFactor"].get_double());

    mvScaleFactors = MongoIO::types::b_vector_float::from(view["mvScaleFactors"]);
    mvLevelSigma2 = MongoIO::types::b_vector_float::from(view["mvLevelSigma2"]);
    mvInvLevelSigma2 = MongoIO::types::b_vector_float::from(view["mvInvLevelSigma2"]);

    mnMinX = view["mnMinX"].get_int32();
    mnMinY = view["mnMinY"].get_int32();
    mnMaxX = view["mnMaxX"].get_int32();
    mnMaxY = view["mnMaxY"].get_int32();
    mK = MongoIO::types::b_mat::from(view["mK"]);

    std::map<uint32_t , double> bow_vec = MongoIO::types::b_bowvec::from(view["mBowVec"]);
    for_each(bow_vec.begin(), bow_vec.end(), [&](std::pair<uint32_t , double> elem){mBowVec.insert(elem);});

    std::map<uint32_t , std::vector<unsigned int> > feat_vec = MongoIO::types::b_bowfeat::from(view["mFeatVec"]);
    for_each(feat_vec.begin(), feat_vec.end(), [&](std::pair<uint32_t , std::vector<unsigned int> > elem){mFeatVec.insert(elem);});

    Tcw = MongoIO::types::b_mat::from(view["Tcw"]);
    mGrid = MongoIO::types::b_grid::from(view["mGrid"]);

    KeyFrame::GetFromDatabase(mConnectedKeyFrameWeights, mpMap, view["mConnectedKeyFrameWeights"]);

    KeyFrame::GetFromDatabase(mvpOrderedConnectedKeyFrames, mpMap, view["mvpOrderedConnectedKeyFrames"]);

    mvOrderedWeights = MongoIO::types::b_vector<int>::from(view["mvOrderedWeights"]);

    mbFirstConnection = view["mbFirstConnection"].get_bool();

    const bsoncxx::types::b_oid pParentID = view["mpParent"].get_oid();
    mpParent = mpMap->GetDatabaseKeyFrame(pParentID);

    KeyFrame::GetFromDatabase(mspChildrens, mpMap, view["mspChildrens"]);

    KeyFrame::GetFromDatabase(mspLoopEdges, mpMap, view["mspLoopEdges"]);

    mvMapPointsID = MongoIO::types::b_vector<bsoncxx::types::b_oid>::from(view["mvpMapPoints"]);

}

    void KeyFrame::Set_Image(const cv::Mat tColorImage, const cv::Mat tDepthImage)
    {
        iColorImg = tColorImage;
        iDepthImg = tDepthImage;
    }

} //namespace ORB_SLAM
